#include "Arm.h"
#define manual 1

#define armGain 0.4
#define liftGain 0.5

Arm::Arm(OperatorInterface *oiArmX)
{
	oiArm = oiArmX;
	
	lift1 = new Jaguar(5);
	lift2 = new Jaguar(6);
	arm = new Jaguar(7);
	
	liftEnc = new Encoder(7,8);
	liftEnc->SetDistancePerPulse(1);
	liftEnc->Start();
	//armEnc = new Encoder(12);//ToDo check the port
	
	gripperSolenoid = new DoubleSolenoid(1,2);//shifter is 2 and 3
	minibotSolenoid = new DoubleSolenoid(7,8);
	brakeSolenoid = new DoubleSolenoid(3,4);
	
	Kpl = 0.00001; //todo tune PID loop
	Kil = Kdl = Kia = Kda = 0.0;
	Kpa = 1.0;
	//liftSetpoints [8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
	//max is -4959
	liftSetpoints[0] = 4959;//top right 113.5"
	liftSetpoints[1] = 4036.5;//middle right 76.5" -4036.5
	liftSetpoints[2] = 1943.25;//bottom right 39.5" -1943.25
	liftSetpoints[3] = 4959;//top left 105" 10" past max
	liftSetpoints[4] = 3594.75;//middle left 68" -3594.75
	liftSetpoints[5] = 1526.75;//bottom left 31" -1526.75
	liftSetpoints[6] = 2067.25;//feeding 42" -2067.25
	liftSetpoints[7] = 0.0;//ground
	
	liftCalc = new PIDCalculator();
	liftPID = new PIDController(Kpl,Kil,Kdl,liftEnc,liftCalc);
	armCalc = new PIDCalculator();
	armPID = new PIDController(Kpa,Kia,Kda,liftEnc,armCalc);//CHANGE INPUT
	//set input range, ouput range, and tolerance for each
	liftPID->SetSetpoint(0);
	liftPID->SetTolerance(15);
	liftPID->SetOutputRange(1,-1);
	liftPID->SetInputRange(0,-4959);

	armPID->SetTolerance(.5);
	armPID->SetOutputRange(-0.5,0.5);
	
	topLiftSwitch = new DigitalInput(11);
}//end of constructor

void Arm::liftInit()
{
	liftPID->Enable();
	liftEnc->Reset();
	liftPID->SetSetpoint(liftSetpoints[7]);
#if liftDebug == 1
	SmartDashboard::init();
#endif
}//end of liftInit()

void Arm::liftSubroutine()
{
	oiArm->updateButtons();
	
	/*********Start Manual Lift Control*********/
#if manual == 1
	if(oiArm->ds->GetDigitalIn(4)==0)
	{
		lift1->Set(liftGain);
		lift2->Set(liftGain));
	}else if(oiArm->ds->GetDigitalIn(2)==0)
	{
		lift1->Set(-liftGain);
		lift2->Set(-liftGain);
	}else{
		lift1->Set(0.0);
		lift2->Set(0.0);
	}
	brakeSolenoid->Set(DoubleSolenoid::kReverse);//or kForward
#endif
	/*********End Manual Lift Control*********/
	
	/*********Start Setpoint Lift Control*********/
#if manual == 0
	if(oiArm->bottomLeft)
	{
		liftPID->SetSetpoint(liftSetpoints[5]);
		printf("bottomLeft");
	}
	else if(oiArm->middleLeft)
	{
		liftPID->SetSetpoint(liftSetpoints[4]);
		printf("middleLeft");
	}
	else if(oiArm->topLeft)
	{
		liftPID->SetSetpoint(liftSetpoints[3]);
		printf("topLeft");
	}
	else if(oiArm->bottomRight)
	{
		liftPID->SetSetpoint(liftSetpoints[2]);
		printf("bottomRight");
	}
	else if(oiArm->middleRight)
	{
		liftPID->SetSetpoint(liftSetpoints[1]);
		printf("middleRight");
	}
	else if(oiArm->topRight)
	{
		liftPID->SetSetpoint(liftSetpoints[0]);
		printf("topRight");
	}else if(oiArm->ground)
	{
		liftPID->SetSetpoint(liftSetpoints[7]);
		printf("ground");
	}else if(oiArm->feeding)
	{
		liftPID->SetSetpoint(liftSetpoints[6]);
		printf("feeding");
	}
	
	lift1->Set((-1)*(liftCalc->getOutput()));
	lift2->Set((-1)*(liftCalc->getOutput()));
	
	brakeSolenoid->Set(DoubleSolenoid::kForward);//or backward
#endif //end of manual == 0
	//printf("LiftEnc: %f\n",liftEnc->GetDistance());
#if liftDebug == 1
	SmartDashboard::Log(liftEnc->GetDistance(),"Encoder Value");
	SmartDashboard::Log(liftPID->GetSetpoint(),"PID Setpoint");
	SmartDashboard::Log(liftPID->GetError(),"PID Error");
	SmartDashboard::Log(liftPID->OnTarget(),"OnTarget");
	SmartDashboard::Log(liftCalc->getOutput(),"PID Output");
	printf("PID Setpoint: %f\n",liftPID->GetSetpoint());
#endif
}//end of liftSubroutine

void Arm::armSubroutine()
{
	oiArm->updateButtons();
#if manual == 0
		if(oiArm->ds->GetDigitalIn(4)==0)
		{
			arm->Set(-0.4);
		}else if(oiArm->ds->GetDigitalIn(2)==0)
		{
			arm->Set(0.4);
		}else{
			arm->Set(0.0);
		}
#endif
#if manual == 1

		if(oiArm->middleRight)
		{
			arm->Set(-armGain);
			//printf("arm down\n");
		}else if(oiArm->topRight)
		{
			arm->Set(armGain);
			//printf("arm up\n");
		}else{
			arm->Set(0.0);
		}
#endif
	
	/*****Gripper pneumatics*****/
	if(oiArm->gripper)
	{
		gripperSolenoid->Set(DoubleSolenoid::kReverse);
	}else if(oiArm->gripper == false)
	{
		gripperSolenoid->Set(DoubleSolenoid::kForward);
	}
	/*****Minibot pneumatics*****/
	
	if(oiArm->mbUp==false)
	{
		minibotSolenoid->Set(DoubleSolenoid::kReverse);
	}else if(oiArm->mbDown==false)
	{
		minibotSolenoid->Set(DoubleSolenoid::kForward);
	}
	// negative values raise the arm
}//end of armSubroutine

Arm::~Arm()
{
	delete lift1;
	delete lift2;
	delete arm;
	delete oiArm;
	
	delete bottomLiftSwitch;
	delete topLiftSwitch;
	delete bottomArmSwitch;
	delete topArmSwitch;
	delete liftCalc;
	delete liftPID;
	delete armCalc;
	delete armPID;
	delete liftEnc;
	delete gripperSolenoid;
	delete minibotSolenoid;
	delete brakeSolenoid;

	
	oiArm = NULL;
	lift1 = NULL;
	lift2 = NULL;
	arm = NULL;

	bottomLiftSwitch = NULL;
	topLiftSwitch = NULL;
	bottomArmSwitch = NULL;
	topArmSwitch = NULL;
	liftCalc = NULL;
	liftPID = NULL;
	armCalc = NULL;
	armPID = NULL;
	liftEnc = NULL;
	gripperSolenoid = NULL;
	minibotSolenoid = NULL;
	brakeSolenoid = NULL;
}
